#include <cmath>
#include "MathUtils.h"

class ModelMatrix {

public:
	float matrix[4][4];

	void loadIdentity() {
		matrix[0][0] = 1.0f; matrix[0][1] = 0.0f; matrix[0][2] = 0.0f; matrix[0][3] = 0.0f;
		matrix[1][0] = 0.0f; matrix[1][1] = 1.0f; matrix[1][2] = 0.0f; matrix[1][3] = 0.0f;
		matrix[2][0] = 0.0f; matrix[2][1] = 0.0f; matrix[2][2] = 1.0f; matrix[2][3] = 0.0f;
		matrix[3][0] = 0.0f; matrix[3][1] = 0.0f; matrix[3][2] = 0.0f; matrix[3][3] = 1.0f;
	}

	ModelMatrix() {
		loadIdentity();
	}

	void multiply(const float rightMatrix[4][4]) {
		float leftMatrix[4][4] = {
			matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3],
			matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
			matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3],
			matrix[3][0], matrix[3][1], matrix[3][2], matrix[3][3]
		};

		for(int i = 0; i < 4; ++i) {
			for (int j = 0; j < 4; ++j) {
				float sum = 0.0f;
				for (int k = 0; k < 4; ++k) {
					sum += leftMatrix[i][k] * rightMatrix[k][j];
				}
				matrix[i][j] = sum;
			}
		}
	}

	void translate(float x, float y, float z) {
		float rightMatrix[4][4] = {
			1.0f, 0.0f, 0.0f, x,
			0.0f, 1.0f, 0.0f, y,
			0.0f, 0.0f, 1.0f, z,
			0.0f, 0.0f, 0.0f, 1.0f
		};

		multiply(rightMatrix);
	}

	void scale(float x, float y, float z){
		float rightMatrix[4][4] = {
			x, 0.0f, 0.0f, 0.0f,
			0.0f, x, 0.0f, 0.0f,
			0.0f, 0.0f, y, 0.0f,
			0.0f, 0.0f, 0.0f, 1.0f
		};
		multiply(rightMatrix);
	}

	void rotate(float angle, float x, float y, float z) {
		float angleCos = cos(angle);
		float OneMinAngleCos = 1.0f - angleCos;
		float angleSin = sin(angle);
		
		normalize(z, y, z);

		float rightMatrix[4][4] = {
			x * x * OneMinAngleCos + angleCos, x * y * OneMinAngleCos - z * angleSin, x * z * OneMinAngleCos + y * angleSin, 0.0f,
			y * x * OneMinAngleCos + z * angleSin, y * y * OneMinAngleCos + angleCos, y * z * OneMinAngleCos - z * angleSin, 0.0f,
			z * x * OneMinAngleCos - y * angleSin, z * y * OneMinAngleCos + x * angleSin, z * z * OneMinAngleCos + angleCos, 0.0f,
			0.0f, 0.0f, 0.0f, 1.0f
		};

		multiply(rightMatrix);
	}
};